﻿using ManhattanProcedure.Model;
using Newtonsoft.Json;
using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.IO.Ports;
using System.IO;
using System.Linq;
using System.Text;
using System.Threading;
using System.Threading.Tasks;
using System.Windows.Interop;

namespace ManhattanProcedure
{
    /// <summary>
    /// 机器人控制
    /// </summary>
    public class RobotOperate
    {
        public static string RobotOperateUrl = AppDomain.CurrentDomain.SetupInformation.ApplicationBase;
        public static int handle1 = 0;
        public static int handle2 = 0;
        public static double PI = 3.1415926535;
        public static List<string[]> listStrArr1 = new List<string[]>();//数组List，相当于可以无限扩大的二维数组。
        public static List<string[]> listStrArr2 = new List<string[]>();
        public static List<string[]> listStrArr3 = new List<string[]>();//数组List，相当于可以无限扩大的二维数组。
        public static List<string[]> listStrArr4 = new List<string[]>();
        public static List<string[]> listStrArr5 = new List<string[]>();//数组List，相当于可以无限扩大的二维数组。
        public static List<string[]> listStrArr6 = new List<string[]>();
        private static RobotOperate instance;
        private RobotOperate() { }
        public static RobotOperate Instance
        {
            get
            {
                if (instance == null)
                {
                    instance = new RobotOperate();
                }
                return instance;
            }
        }
        public static JKTYPE.ProgramState pstatusR;
        public static JKTYPE.ProgramState pstatusL;
        public static JKTYPE.RobotState stateL;
        public static JKTYPE.RobotStatus statusL;
        public static JKTYPE.RobotState stateR;
        public static JKTYPE.RobotStatus statusR;
        public static void CX_Start_L()
        {
            try
            {
                jakaAPI.get_program_state(ref handle1, ref pstatusL);
            }
            catch (Exception)
            {
                return;
            }
        }
        public static void CX_kj_L()
        {
            try
            {
                jakaAPI.get_robot_state(ref handle1, ref stateL);
            }
            catch (Exception)
            {
                return;
            }
        }
        public static void CX_status_L()
        {
            try
            {
                jakaAPI.get_robot_status(ref handle1, ref statusL);
            }
            catch (Exception)
            {
                return;
            }
        }
        public static void CX_Start_R()
        {
            try
            {
                jakaAPI.get_program_state(ref handle2, ref pstatusR);
            }
            catch (Exception)
            {
                return;
            }
        }
        public static void CX_kj_R()
        {
            try
            {
                jakaAPI.get_robot_state(ref handle2, ref stateR);
            }
            catch (Exception)
            {
                return;
            }
        }
        public static void CX_status_R()
        {
            try
            {
                jakaAPI.get_robot_status(ref handle2, ref statusR);
            }
            catch (Exception)
            {
                return;
            }
        }
        public static void CX_Stop()
        {
            try
            {
                jakaAPI.program_abort(ref handle1);
            }
            catch (Exception)
            {
                return;
            }
        }
        public static void CX_Stop_R()
        {
            try
            {
                jakaAPI.program_abort(ref handle2);
            }
            catch (Exception)
            {
                return;
            }
        }
        public static void Robot_Yun_R()
        {
            try
            {
                jakaAPI.program_run(ref handle2);
            }
            catch (Exception)
            {
                return;
            }
        }
        public static void Read_CSV()//读取轨迹文件
        {
            string line1 = "";
            string line2 = "";
            string line3 = "";
            string line4 = "";
            string line5 = "";
            string line6 = "";
            string path1 = RobotOperateUrl + "\\cup_swan_single1.csv";
            StreamReader reader1 = new StreamReader(path1);

            line1 = reader1.ReadLine();
            while (line1 != null)
            {
                listStrArr1.Add(line1.Split(','));//将文件内容分割成数组
                line1 = reader1.ReadLine();
            }
            reader1.Close();

            StreamReader reader2 = new StreamReader(RobotOperateUrl + "/milk_swan_single_adj4.csv");
            line2 = reader2.ReadLine();
            while (line2 != null)
            {
                listStrArr2.Add(line2.Split(','));//将文件内容分割成数组
                line2 = reader2.ReadLine();
            }
            reader2.Close();
            StreamReader reader3 = new StreamReader(RobotOperateUrl + "/cup_tulip2.csv");
            line3 = reader3.ReadLine();
            while (line3 != null)
            {
                listStrArr3.Add(line3.Split(','));//将文件内容分割成数组
                line3 = reader3.ReadLine();
            }
            reader3.Close();

            StreamReader reader4 = new StreamReader(RobotOperateUrl + "/milk_tulip3.csv");
            line4 = reader4.ReadLine();
            while (line4 != null)
            {
                listStrArr4.Add(line4.Split(','));//将文件内容分割成数组
                line4 = reader4.ReadLine();
            }
            reader4.Close();
            StreamReader reader5 = new StreamReader(RobotOperateUrl + "/cup_malaysia_tulip.csv");
            line5 = reader5.ReadLine();
            while (line5 != null)
            {
                listStrArr5.Add(line5.Split(','));//将文件内容分割成数组
                line5 = reader5.ReadLine();
            }
            reader5.Close();
            StreamReader reader6 = new StreamReader(RobotOperateUrl + "/milk_malaysia_tulip.csv");
            line6 = reader6.ReadLine();
            while (line6 != null)
            {
                listStrArr6.Add(line6.Split(','));//将文件内容分割成数组
                line6 = reader6.ReadLine();
            }
            reader6.Close();
        }
        public void Robot_Initialisation_double()//机器人初始化
        {
            CoffeeIOMsg.Instance.AllBol = false;
            Thread.Sleep(200);
            CoffeeIOSerialPort.Instance.SerialSend(CoffeeIOModel.Instance.RobotPowerOn_start);
            Thread.Sleep(3000);
            CoffeeIOSerialPort.Instance.SerialSend(CoffeeIOModel.Instance.RobotPowerOn2_start);
            Thread.Sleep(500);
            int tool_idset1 = 0;
            int frame_idset1 = 0;
            int tool_idset2 = 0;
            int frame_idset2 = 0;
            int result1 = -1;
            int result2 = -1;
            while (result1 == -1)
            {
                result1 = jakaAPI.create_handler(IPorPortInfo.Instance.RobotCoffeeIPLeft.ToCharArray(), ref handle1);
                Thread.Sleep(100);
            }
            while (result2 == -1)
            {
                result2 = jakaAPI.create_handler(IPorPortInfo.Instance.RobotCoffeeIPRight.ToCharArray(), ref handle2);
                Thread.Sleep(100);
            }
            CoffeeIOMsg.Instance.AllBol = false;
            Thread.Sleep(100);
            CoffeeIOSerialPort.Instance.SerialSend(CoffeeIOModel.Instance.RobotPowerOn_end);
            Thread.Sleep(100);
            CoffeeIOSerialPort.Instance.SerialSend(CoffeeIOModel.Instance.RobotPowerOn2_end);
            Thread.Sleep(100);
            CoffeeIOMsg.Instance.AllBol = true;
            System.Threading.Thread.Sleep(3000); //毫秒
            //机器人上电
            jakaAPI.power_on(ref handle1); Console.WriteLine("L_Robot Power On");
            //机器人上电
            jakaAPI.power_on(ref handle2); Console.WriteLine("R_Robot Power On");
            //切换当前使用的工具坐标
            jakaAPI.set_tool_id(ref handle1, tool_idset1); Console.WriteLine("L_Robot_T " + tool_idset1);
            //切换当前使用的世界坐标
            jakaAPI.set_user_frame_id(ref handle1, frame_idset1); Console.WriteLine("L_Robot_F " + frame_idset1);

            //切换当前使用的工具坐标
            jakaAPI.set_tool_id(ref handle2, tool_idset2); Console.WriteLine("R_Robot_T " + tool_idset2);
            //切换当前使用的世界坐标
            jakaAPI.set_user_frame_id(ref handle2, frame_idset2); Console.WriteLine("R_Robot_F " + frame_idset2);
            StringBuilder Version = new StringBuilder("", 3000);
            int result3 = jakaAPI.get_sdk_version(ref handle1, Version);
            Console.WriteLine("SDK 版本 " + Version);
            CoffeeIOMsg.Instance.AllBol = false;
            Thread.Sleep(100);
            CoffeeIOMsg.Instance.QK();
            Thread.Sleep(100);
            CoffeeIOMsg.Instance.AllBol = true;
            ClientList clientList2 = new ClientList();
            clientList2.MsgBol = WuLiaoCoffeeSel.Instance;
            clientList2.code = 12;
            SocketClients.Instance.client.Send(Encoding.UTF8.GetBytes(JsonConvert.SerializeObject(clientList2)));
            //Thread thr2 = new Thread(MainWindow.MessageShow);
            //thr2.IsBackground = true;
            //thr2.Start();
        }
        public void Robot_L_Show()
        {
            CX_status_L();
            CX_Start_L();
            if (statusL.powered_on == 1)
                RobotMsgs(false, true, false, false, false, false, false);
            if (statusL.enabled == 1)
                RobotMsgs(false, false, true, false, false, false, false);
            if (pstatusL == JKTYPE.ProgramState.PROGRAM_RUNNING)
                RobotMsgs(false, false, false, true, false, false, false);
            if (pstatusL == JKTYPE.ProgramState.PROGRAM_IDLE)
                RobotMsgs(false, false, false, false, true, false, false);
            if (pstatusL == JKTYPE.ProgramState.PROGRAM_PAUSED)
                RobotMsgs(false, false, false, false, false, true, false);
            if (statusL.enabled == 0)
                RobotMsgs(false, true, false, false, false, false, false);
            if (statusL.powered_on == 0)
                RobotMsgs(true, false, false, false, false, false, false);
            if (statusL.errcode != 0)
                RobotMsgs(false, false, false, false, false, false, true);
        }
        public void Robot_R_Show()
        {
            CX_status_R();
            CX_Start_R();
            if (statusR.powered_on == 1)
                RobotMsgs(false, true, false, false, false, false, false, 1);
            if (statusR.enabled == 1)
                RobotMsgs(false, false, true, false, false, false, false, 1);
            if (pstatusR == JKTYPE.ProgramState.PROGRAM_RUNNING)
                RobotMsgs(false, false, false, true, false, false, false, 1);
            if (pstatusR == JKTYPE.ProgramState.PROGRAM_IDLE)
                RobotMsgs(false, false, false, false, true, false, false, 1);
            if (pstatusR == JKTYPE.ProgramState.PROGRAM_PAUSED)
                RobotMsgs(false, false, false, false, false, true, false, 1);
            if (statusR.enabled == 0)
                RobotMsgs(false, true, false, false, false, false, false, 1);
            if (statusR.powered_on == 0)
                RobotMsgs(true, false, false, false, false, false, false, 1);
            if (statusR.errcode != 0)
                RobotMsgs(false, false, false, false, false, false, true, 1);
        }

        /// <summary>
        /// 机器人状态
        /// </summary>
        /// <param name="KJ">开机</param>
        /// <param name="SD">上电</param>
        /// <param name="SN">使能</param>
        /// <param name="YX">运行</param>
        /// <param name="TZ">停止</param>
        /// <param name="ZT">暂停</param>
        /// <param name="BJ">报警</param>
        ///  <param name="type">左右手 0 左 1 右</param>
        public void RobotMsgs(bool KJ, bool SD, bool SN, bool YX, bool TZ, bool ZT, bool BJ, int type = 0)
        {
            RobotMsg.Instance.Robot_YKJ = KJ;
            RobotMsg.Instance.Robot_SD = SD;
            RobotMsg.Instance.Robot_SN = SN;
            RobotMsg.Instance.Robot_YX = YX;
            RobotMsg.Instance.Robot_TZ = TZ;
            RobotMsg.Instance.Robot_ZT = ZT;
            RobotMsg.Instance.Robot_BJ = BJ;
        }
        public void Robot_Initial_Point()//通过关节直线到初始点
        {
            //定义并初始化关JointValue变量
            JKTYPE.JointValue joint_pos1 = new JKTYPE.JointValue();
            JKTYPE.JointValue joint_pos11 = new JKTYPE.JointValue();
            JKTYPE.JointValue joint_pos2 = new JKTYPE.JointValue();
            JKTYPE.JointValue joint_pos22 = new JKTYPE.JointValue();
            //yjx_1J_412 yjx_2J_412
            joint_pos11.jVal = new double[] { 262.227 / 57.3, -43.297 / 57.3, -104.307 / 57.3, 121.504 / 57.3, -73.785 / 57.3, 117.196 / 57.3 };//初始
            joint_pos1.jVal = new double[] { 7.160 / 57.3, 27.992 / 57.3, 119.469 / 57.3, -142.118 / 57.3, 62.838 / 57.3, 159.148 / 57.3 };
            joint_pos2.jVal = new double[] { 292.580 / 57.3, -15.393 / 57.3, -122.816 / 57.3, 110.422 / 57.3, -66.025 / 57.3, 136.822 / 57.3 };
            joint_pos22.jVal = new double[] { 48.915 / 57.3, 29.870 / 57.3, 117.332 / 57.3, -161.063 / 57.3, 58.664 / 57.3, 169.889 / 57.3 };
            Console.WriteLine("joint_move_开始");
            //阻塞运动到目标点位
            jakaAPI.joint_move(ref handle1, ref joint_pos22, JKTYPE.MoveMode.ABS, true, 1);
            jakaAPI.joint_move(ref handle1, ref joint_pos1, JKTYPE.MoveMode.ABS, true, 1);
            jakaAPI.joint_move(ref handle2, ref joint_pos11, JKTYPE.MoveMode.ABS, true, 1);
            jakaAPI.joint_move(ref handle2, ref joint_pos2, JKTYPE.MoveMode.ABS, true, 1);
            //System.Threading.Thread.Sleep(1500); //毫秒
            Console.WriteLine("joint_move_结束");
            //linear_move指令执行开始、
            Console.WriteLine("linear_move_开始");
            JKTYPE.CartesianPose tcp_pos1 = new JKTYPE.CartesianPose();
            JKTYPE.CartesianPose tcp_pos2 = new JKTYPE.CartesianPose();
            //yjx_1J_412   yjx_2J_412
            tcp_pos1.tran.x = 37.274; tcp_pos1.tran.y = 352.718; tcp_pos1.tran.z = 192.557;
            tcp_pos1.rpy.rx = 104.960 / 57.3; tcp_pos1.rpy.ry = -88.416 / 57.3; tcp_pos1.rpy.rz = 139.090 / 57.3;
            tcp_pos2.tran.x = -205.981; tcp_pos2.tran.y = -226.677; tcp_pos2.tran.z = 217.933;
            tcp_pos2.rpy.rx = -95.242 / 57.3; tcp_pos2.rpy.ry = 7.952 / 57.3; tcp_pos2.rpy.rz = 171.149 / 57.3;

            jakaAPI.linear_move(ref handle1, ref tcp_pos1, JKTYPE.MoveMode.ABS, true, 30);
            jakaAPI.linear_move(ref handle2, ref tcp_pos2, JKTYPE.MoveMode.ABS, true, 30);
            Console.WriteLine("linear_move_结束");
        }
        public static void Robot_Yun()
        {
            try
            {
                jakaAPI.program_run(ref handle1);
            }
            catch (Exception)
            {
                return;
            }
        }
        /// <summary>
        /// 天鹅2-机器人伺服关节空间运动
        /// </summary>
        public static void Robot_servo_j_Swan2()//天鹅2-机器人伺服关节空间运动
        {
            Robot_Filter_LPF(3, 10);
            JKTYPE.JointValue[] joint_pos1 = new JKTYPE.JointValue[5000];
            JKTYPE.JointValue[] joint_pos2 = new JKTYPE.JointValue[5000];

            int length = listStrArr1.Count;

            Stopwatch sw = new Stopwatch();//计时器

            for (int i = 0; i < length; i++)
            {
                double[] jointTarget1 = new double[6];
                double[] jointTarget2 = new double[6];

                for (int j = 0; j < 6; j++)
                {
                    jointTarget1[j] = Convert.ToDouble(listStrArr1[i][j]);
                    jointTarget2[j] = Convert.ToDouble(listStrArr2[i][j]);
                    //jakaAPI.servo_j(ref handle1, ref joint_pos1, JKTYPE.MoveMode.ABS);
                }
                joint_pos1[i].jVal = jointTarget1;
                joint_pos2[i].jVal = jointTarget2;
            }
            Console.WriteLine("joint_move");
            jakaAPI.joint_move(ref handle1, ref joint_pos1[0], JKTYPE.MoveMode.ABS, false, 30);
            jakaAPI.joint_move(ref handle2, ref joint_pos2[0], JKTYPE.MoveMode.ABS, true, 30);

            //TRUE为进入伺服模式
            jakaAPI.servo_move_enable(ref handle1, true);
            jakaAPI.servo_move_enable(ref handle2, true);
            //定义并初始化关CartesianPose变量
            System.Threading.Thread.Sleep(300); //毫秒

            Console.WriteLine("servo_j_开始");
            sw.Start();

            for (int i = 0; i < length; i++)
            {
                jakaAPI.servo_j(ref handle1, ref joint_pos1[i], JKTYPE.MoveMode.ABS);
                jakaAPI.servo_j(ref handle2, ref joint_pos2[i], JKTYPE.MoveMode.ABS);
                //jakaAPI.servo_j_extend(ref handle1, ref joint_pos1[i], JKTYPE.MoveMode.ABS,3);
            }
            System.Threading.Thread.Sleep(200);

            //FALSE为退出伺服模式
            jakaAPI.servo_move_enable(ref handle1, false);
            jakaAPI.servo_move_enable(ref handle2, false);
            sw.Stop();
            Console.WriteLine("servo_j_结束{0}s", sw.ElapsedTicks / (decimal)Stopwatch.Frequency);
        }
        /// <summary>
        /// 叶子
        /// </summary>
        public static void Robot_servo_j_Leaf()//树叶-机器人伺服关节空间运动
        {
            Robot_Filter_LPF(0.5, 10);//滤波器设置
            JKTYPE.JointValue[] joint_pos1 = new JKTYPE.JointValue[5000];
            JKTYPE.JointValue[] joint_pos2 = new JKTYPE.JointValue[5000];

            int length = listStrArr3.Count;

            Stopwatch sw = new Stopwatch();//计时器

            for (int i = 0; i < length; i++)
            {
                double[] jointTarget1 = new double[6];
                double[] jointTarget2 = new double[6];

                for (int j = 0; j < 6; j++)
                {
                    jointTarget1[j] = Convert.ToDouble(listStrArr3[i][j]);
                    jointTarget2[j] = Convert.ToDouble(listStrArr4[i][j]);
                    //jakaAPI.servo_j(ref handle1, ref joint_pos1, JKTYPE.MoveMode.ABS);
                }
                joint_pos1[i].jVal = jointTarget1;
                joint_pos2[i].jVal = jointTarget2;
            }
            Console.WriteLine("joint_move");
            jakaAPI.joint_move(ref handle1, ref joint_pos1[0], JKTYPE.MoveMode.ABS, false, 30);
            jakaAPI.joint_move(ref handle2, ref joint_pos2[0], JKTYPE.MoveMode.ABS, true, 30);

            //TRUE为进入伺服模式
            jakaAPI.servo_move_enable(ref handle1, true);
            jakaAPI.servo_move_enable(ref handle2, true);
            //定义并初始化关CartesianPose变量
            System.Threading.Thread.Sleep(300); //毫秒

            Console.WriteLine("servo_j_开始");
            sw.Start();

            for (int i = 0; i < length; i++)
            {
                jakaAPI.servo_j(ref handle1, ref joint_pos1[i], JKTYPE.MoveMode.ABS);
                jakaAPI.servo_j(ref handle2, ref joint_pos2[i], JKTYPE.MoveMode.ABS);
                //jakaAPI.servo_j_extend(ref handle1, ref joint_pos1[i], JKTYPE.MoveMode.ABS,3);
            }
            System.Threading.Thread.Sleep(200);

            //FALSE为退出伺服模式
            jakaAPI.servo_move_enable(ref handle1, false);
            jakaAPI.servo_move_enable(ref handle2, false);
            sw.Stop();
            Console.WriteLine("servo_j_结束{0}s", sw.ElapsedTicks / (decimal)Stopwatch.Frequency);
        }
        public static void Robot_servo_j_Tulip()//郁金香2-机器人伺服关节空间运动
        {
            Robot_Filter_LPF(3, 10);//滤波器设置
            JKTYPE.JointValue[] joint_pos1 = new JKTYPE.JointValue[5000];
            JKTYPE.JointValue[] joint_pos2 = new JKTYPE.JointValue[5000];

            int length = listStrArr5.Count;

            Stopwatch sw = new Stopwatch();//计时器

            for (int i = 0; i < length; i++)
            {
                double[] jointTarget1 = new double[6];
                double[] jointTarget2 = new double[6];

                for (int j = 0; j < 6; j++)
                {
                    jointTarget1[j] = Convert.ToDouble(listStrArr5[i][j]);
                    jointTarget2[j] = Convert.ToDouble(listStrArr6[i][j]);
                    //jakaAPI.servo_j(ref handle1, ref joint_pos1, JKTYPE.MoveMode.ABS);
                }
                joint_pos1[i].jVal = jointTarget1;
                joint_pos2[i].jVal = jointTarget2;
            }
            Console.WriteLine("joint_move");
            jakaAPI.joint_move(ref handle1, ref joint_pos1[0], JKTYPE.MoveMode.ABS, false, 30);
            jakaAPI.joint_move(ref handle2, ref joint_pos2[0], JKTYPE.MoveMode.ABS, true, 30);

            //TRUE为进入伺服模式
            jakaAPI.servo_move_enable(ref handle1, true);
            jakaAPI.servo_move_enable(ref handle2, true);
            //定义并初始化关CartesianPose变量
            System.Threading.Thread.Sleep(300); //毫秒

            Console.WriteLine("servo_j_开始");
            sw.Start();

            for (int i = 0; i < length; i++)
            {
                jakaAPI.servo_j(ref handle1, ref joint_pos1[i], JKTYPE.MoveMode.ABS);
                jakaAPI.servo_j(ref handle2, ref joint_pos2[i], JKTYPE.MoveMode.ABS);
                //jakaAPI.servo_j_extend(ref handle1, ref joint_pos1[i], JKTYPE.MoveMode.ABS,3);
            }
            System.Threading.Thread.Sleep(200);

            //FALSE为退出伺服模式
            jakaAPI.servo_move_enable(ref handle1, false);
            jakaAPI.servo_move_enable(ref handle2, false);
            sw.Stop();
            Console.WriteLine("servo_j_结束{0}s", sw.ElapsedTicks / (decimal)Stopwatch.Frequency);
        }
        public static void Robot_Move(int Action)//机器人运动
        {
            //需要坐标点可添加
            JKTYPE.JointValue joint_pos_L1 = new JKTYPE.JointValue();//左臂初始点
            JKTYPE.JointValue joint_pos_L2 = new JKTYPE.JointValue();//拉花准备
            JKTYPE.JointValue joint_pos_L3 = new JKTYPE.JointValue();//拉花准备

            JKTYPE.JointValue joint_pos_R1 = new JKTYPE.JointValue();//右臂初始点
            JKTYPE.JointValue joint_pos_R2 = new JKTYPE.JointValue();//右臂倒奶1
            JKTYPE.JointValue joint_pos_R3 = new JKTYPE.JointValue();//右臂倒奶3
            JKTYPE.JointValue joint_pos_R4 = new JKTYPE.JointValue();//右臂过度

            JKTYPE.CartesianPose cartesianPose_L1 = new JKTYPE.CartesianPose();//取杯过度点1
            JKTYPE.CartesianPose cartesianPose_L2 = new JKTYPE.CartesianPose();//取杯过度点2
            JKTYPE.CartesianPose cartesianPose_L3 = new JKTYPE.CartesianPose();//取杯准备
            JKTYPE.CartesianPose cartesianPose_L4 = new JKTYPE.CartesianPose();//取杯
            JKTYPE.CartesianPose cartesianPose_L5 = new JKTYPE.CartesianPose();//取杯上
            JKTYPE.CartesianPose cartesianPose_L6 = new JKTYPE.CartesianPose();//接咖啡下
            JKTYPE.CartesianPose cartesianPose_L61 = new JKTYPE.CartesianPose();//接咖啡上
            JKTYPE.CartesianPose cartesianPose_L7 = new JKTYPE.CartesianPose();//打印上
            JKTYPE.CartesianPose cartesianPose_L8 = new JKTYPE.CartesianPose();//打印下
            JKTYPE.CartesianPose cartesianPose_L9 = new JKTYPE.CartesianPose();//打印后撤
            JKTYPE.CartesianPose cartesianPose_L10 = new JKTYPE.CartesianPose();//出杯过渡
            JKTYPE.CartesianPose cartesianPose_L11 = new JKTYPE.CartesianPose();//出杯上
            JKTYPE.CartesianPose cartesianPose_L12 = new JKTYPE.CartesianPose();//出杯下
            JKTYPE.CartesianPose cartesianPose_L13 = new JKTYPE.CartesianPose();//出杯后撤
            JKTYPE.CartesianPose cartesianPose_L14 = new JKTYPE.CartesianPose();//初始点
            JKTYPE.CartesianPose cartesianPose_L15 = new JKTYPE.CartesianPose();//杯检测
            JKTYPE.CartesianPose cartesianPose_L16 = new JKTYPE.CartesianPose();//只接咖啡 

            JKTYPE.CartesianPose cartesianPose_R1 = new JKTYPE.CartesianPose();//右臂接奶准备
            JKTYPE.CartesianPose cartesianPose_R2 = new JKTYPE.CartesianPose();//右臂接奶下  
            JKTYPE.CartesianPose cartesianPose_R3 = new JKTYPE.CartesianPose();//右臂接奶上  
            JKTYPE.CartesianPose cartesianPose_R4 = new JKTYPE.CartesianPose();//右臂倒奶
            JKTYPE.CartesianPose cartesianPose_R5 = new JKTYPE.CartesianPose();//右臂洗奶上
            JKTYPE.CartesianPose cartesianPose_R6 = new JKTYPE.CartesianPose();//右臂洗奶下
            JKTYPE.CartesianPose cartesianPose_R7 = new JKTYPE.CartesianPose();//右臂初始点
            JKTYPE.CartesianPose cartesianPose_R8 = new JKTYPE.CartesianPose();//右臂倒奶准备
            JKTYPE.CartesianPose cartesianPose_R9 = new JKTYPE.CartesianPose();//右臂倒奶2mo

            cartesianPose_L1.rpy.rx = 0; cartesianPose_L1.rpy.ry = -90 / 57.26; cartesianPose_L1.rpy.rz = 0;
            cartesianPose_L1.tran.x = -120.000; cartesianPose_L1.tran.y = -360.000; cartesianPose_L1.tran.z = 200.000;
            cartesianPose_L2.rpy.rx = 90 / 57.26; cartesianPose_L2.rpy.ry = -90 / 57.26; cartesianPose_L2.rpy.rz = 0;
            cartesianPose_L2.tran.x = -240; cartesianPose_L2.tran.y = -230; cartesianPose_L2.tran.z = 200;
            cartesianPose_L3.rpy.rx = 120.665 / 57.26; cartesianPose_L3.rpy.ry = -90 / 57.26; cartesianPose_L3.rpy.rz = 0;
            cartesianPose_L3.tran.x = -254.747; cartesianPose_L3.tran.y = -135.504; cartesianPose_L3.tran.z = 77;
            cartesianPose_L4.rpy.rx = 120.665 / 57.26; cartesianPose_L4.rpy.ry = -90 / 57.26; cartesianPose_L4.rpy.rz = 0;
            cartesianPose_L4.tran.x = -176.392; cartesianPose_L4.tran.y = -267.652; cartesianPose_L4.tran.z = 77;
            cartesianPose_L5.rpy.rx = 120.665 / 57.26; cartesianPose_L5.rpy.ry = -90 / 57.26; cartesianPose_L5.rpy.rz = 0;
            cartesianPose_L5.tran.x = -176.392; cartesianPose_L5.tran.y = -267.652; cartesianPose_L5.tran.z = 189.248;
            //cartesianPose_L6.rpy.rx = 41.5 / 57.26; cartesianPose_L6.rpy.ry = -90 / 57.26; cartesianPose_L6.rpy.rz = 0;
            //cartesianPose_L6.tran.x = -350; cartesianPose_L6.tran.y = -416; cartesianPose_L6.tran.z = 253;
            cartesianPose_L6.rpy.rx = 107.638 / 57.26; cartesianPose_L6.rpy.ry = -75.942 / 57.26; cartesianPose_L6.rpy.rz = -79.193 / 57.26;
            cartesianPose_L6.tran.x = -331.533; cartesianPose_L6.tran.y = -412.822; cartesianPose_L6.tran.z = 286.221;
            //cartesianPose_L61.rpy.rx = 109.821 / 57.26; cartesianPose_L61.rpy.ry = -70.842 / 57.26; cartesianPose_L61.rpy.rz = -84.106/57.26;
            //cartesianPose_L61.tran.x = -334.155; cartesianPose_L61.tran.y = -410.431; cartesianPose_L61.tran.z = 301.701;
            cartesianPose_L7.rpy.rx = 80 / 57.26; cartesianPose_L7.rpy.ry = -90 / 57.26; cartesianPose_L7.rpy.rz = 0;
            cartesianPose_L7.tran.x = -230; cartesianPose_L7.tran.y = -506; cartesianPose_L7.tran.z = 160;
            cartesianPose_L8.rpy.rx = 80 / 57.26; cartesianPose_L8.rpy.ry = -90 / 57.26; cartesianPose_L8.rpy.rz = 0;
            cartesianPose_L8.tran.x = -230; cartesianPose_L8.tran.y = -506; cartesianPose_L8.tran.z = 100;
            cartesianPose_L9.rpy.rx = 80 / 57.26; cartesianPose_L9.rpy.ry = -90 / 57.26; cartesianPose_L9.rpy.rz = 0;
            cartesianPose_L9.tran.x = -200; cartesianPose_L9.tran.y = -340; cartesianPose_L9.tran.z = 100;
            cartesianPose_L10.rpy.rx = -50 / 57.26; cartesianPose_L10.rpy.ry = -90 / 57.26; cartesianPose_L10.rpy.rz = 0;
            cartesianPose_L10.tran.x = -240; cartesianPose_L10.tran.y = -140; cartesianPose_L10.tran.z = 230;
            cartesianPose_L11.rpy.rx = -60 / 57.26; cartesianPose_L11.rpy.ry = -90 / 57.26; cartesianPose_L11.rpy.rz = 0;
            cartesianPose_L11.tran.x = -375; cartesianPose_L11.tran.y = 55; cartesianPose_L11.tran.z = 136;
            cartesianPose_L12.rpy.rx = -60 / 57.26; cartesianPose_L12.rpy.ry = -90 / 57.26; cartesianPose_L12.rpy.rz = 0;
            cartesianPose_L12.tran.x = -375; cartesianPose_L12.tran.y = 55; cartesianPose_L12.tran.z = 93.4;
            cartesianPose_L13.rpy.rx = -60 / 57.26; cartesianPose_L13.rpy.ry = -90 / 57.26; cartesianPose_L13.rpy.rz = 0;
            cartesianPose_L13.tran.x = -277; cartesianPose_L13.tran.y = -114.74; cartesianPose_L13.tran.z = 93.4;
            cartesianPose_L14.rpy.rx = 0; cartesianPose_L14.rpy.ry = -90 / 57.26; cartesianPose_L14.rpy.rz = 0;
            cartesianPose_L14.tran.x = -200; cartesianPose_L14.tran.y = -220.000; cartesianPose_L14.tran.z = 240.000;
            cartesianPose_L15.rpy.rx = 108 / 57.26; cartesianPose_L15.rpy.ry = -90 / 57.26; cartesianPose_L15.rpy.rz = 0;
            cartesianPose_L15.tran.x = -215; cartesianPose_L15.tran.y = -256; cartesianPose_L15.tran.z = 184;
            cartesianPose_L16.rpy.rx = -9.431 / 57.26; cartesianPose_L16.rpy.ry = -90 / 57.26; cartesianPose_L16.rpy.rz = 37.858 / 57.26;
            cartesianPose_L16.tran.x = -413.051; cartesianPose_L16.tran.y = -352.571; cartesianPose_L16.tran.z = 236.041;

            cartesianPose_R1.rpy.rx = -90 / 57.26; cartesianPose_R1.rpy.ry = 0; cartesianPose_R1.rpy.rz = -123 / 57.26;
            cartesianPose_R1.tran.x = 240; cartesianPose_R1.tran.y = -397; cartesianPose_R1.tran.z = 226;
            cartesianPose_R2.rpy.rx = -100 / 57.26; cartesianPose_R2.rpy.ry = 10 / 57.26; cartesianPose_R2.rpy.rz = -153 / 57.26;
            cartesianPose_R2.tran.x = 200; cartesianPose_R2.tran.y = -454; cartesianPose_R2.tran.z = 223;
            cartesianPose_R3.rpy.rx = -100 / 57.26; cartesianPose_R3.rpy.ry = 10 / 57.26; cartesianPose_R3.rpy.rz = -153 / 57.26;
            cartesianPose_R3.tran.x = 200; cartesianPose_R3.tran.y = -454; cartesianPose_R3.tran.z = 264;
            cartesianPose_R4.rpy.rx = 90 / 57.26; cartesianPose_R4.rpy.ry = -63 / 57.26; cartesianPose_R4.rpy.rz = 90 / 57.26;
            cartesianPose_R4.tran.x = 318; cartesianPose_R4.tran.y = -221; cartesianPose_R4.tran.z = 241;
            cartesianPose_R5.rpy.rx = 90 / 57.26; cartesianPose_R5.rpy.ry = 0; cartesianPose_R5.rpy.rz = 90 / 57.26;
            cartesianPose_R5.tran.x = 309; cartesianPose_R5.tran.y = -110; cartesianPose_R5.tran.z = 190;
            cartesianPose_R6.rpy.rx = 90 / 57.26; cartesianPose_R6.rpy.ry = 0; cartesianPose_R6.rpy.rz = 90 / 57.26;
            cartesianPose_R6.tran.x = 309; cartesianPose_R6.tran.y = -110; cartesianPose_R6.tran.z = 85;
            cartesianPose_R7.rpy.rx = -90 / 57.26; cartesianPose_R7.rpy.ry = 0; cartesianPose_R7.rpy.rz = -90 / 57.26;
            cartesianPose_R7.tran.x = 250; cartesianPose_R7.tran.y = -210; cartesianPose_R7.tran.z = 245;
            cartesianPose_R8.rpy.rx = -84 / 57.26; cartesianPose_R8.rpy.ry = -50 / 57.26; cartesianPose_R8.rpy.rz = -101 / 57.26;
            cartesianPose_R8.tran.x = 297; cartesianPose_R8.tran.y = -347; cartesianPose_R8.tran.z = 330;
            cartesianPose_R9.rpy.rx = 94 / 57.26; cartesianPose_R9.rpy.ry = -63 / 57.26; cartesianPose_R9.rpy.rz = 89 / 57.26;
            cartesianPose_R9.tran.x = 318; cartesianPose_R9.tran.y = -221; cartesianPose_R9.tran.z = 241;

            joint_pos_L1.jVal = new double[] { 78.016 / 57.26, 19.474 / 57.26, 113.822 / 57.26, -98.282 / 57.26, 81.308 / 57.26, 133.927 / 57.26 };
            joint_pos_L2.jVal = new double[] { 3.5 / 57.26, 22 / 57.26, 113 / 57.26, -141.5 / 57.26, 53 / 57.26, 154 / 57.26 };
            joint_pos_L3.jVal = new double[] { 74.857 / 57.26, 27.871 / 57.26, 118.375 / 57.26, -138.722 / 57.26, 63.334 / 57.26, 158.497 / 57.26 };

            joint_pos_R1.jVal = new double[] { 294.813 / 57.26, -19.564 / 57.26, -111.884 / 57.26, 107.017 / 57.26, -71.666 / 57.26, 135.785 / 57.26 };
            joint_pos_R2.jVal = new double[] { 299.187 / 57.26, -45.514 / 57.26, -72.917 / 57.26, 104.892 / 57.26, -64.606 / 57.26, 58.195 / 57.26 };
            joint_pos_R3.jVal = new double[] { 306.909 / 57.26, -29.706 / 57.26, -97.692 / 57.26, 114.521 / 57.26, -61.504 / 57.26, 19.285 / 57.26 };
            joint_pos_R4.jVal = new double[] { 293.824 / 57.26, -44.326 / 57.26, -50.446 / 57.26, 97.189 / 57.26, -60.339 / 57.26, 122.126 / 57.26 };

            int sd = 85;
            //var ddd=jakaAPI.joint_move(ref handle2, ref joint_pos_R1, JKTYPE.MoveMode.ABS, true, 1);//初始点
            if (Action == 0)//左臂回初始点
            {
                //jakaAPI.linear_move(ref handle1, ref cartesianPose_L14, JKTYPE.MoveMode.ABS, true, 30);
                jakaAPI.joint_move(ref handle1, ref joint_pos_L1, JKTYPE.MoveMode.ABS, false, 1);//初始点
            }
            else if (Action == 1)//去取杯2
            {
                jakaAPI.linear_move(ref handle1, ref cartesianPose_L1, JKTYPE.MoveMode.ABS, true, sd);
                jakaAPI.linear_move(ref handle1, ref cartesianPose_L2, JKTYPE.MoveMode.ABS, true, sd);
                jakaAPI.linear_move(ref handle1, ref cartesianPose_L3, JKTYPE.MoveMode.ABS, true, sd);
                jakaAPI.linear_move(ref handle1, ref cartesianPose_L4, JKTYPE.MoveMode.ABS, true, sd);
                jakaAPI.linear_move(ref handle1, ref cartesianPose_L5, JKTYPE.MoveMode.ABS, true, sd);

            }
            else if (Action == 2)//取杯1
            {
            }
            else if (Action == 3)//取杯成功检测
            {
                jakaAPI.linear_move(ref handle1, ref cartesianPose_L15, JKTYPE.MoveMode.ABS, true, sd);
            }
            else if (Action == 4)//接咖啡
            {
                //jakaAPI.linear_move(ref handle1, ref cartesianPose_L2, JKTYPE.MoveMode.ABS, false, sd);
                //jakaAPI.linear_move(ref handle1, ref cartesianPose_L1, JKTYPE.MoveMode.ABS, false, sd);
                //jakaAPI.linear_move(ref handle1, ref cartesianPose_L14, JKTYPE.MoveMode.ABS, true, sd);//初始点
                jakaAPI.linear_move(ref handle1, ref cartesianPose_L6, JKTYPE.MoveMode.ABS, false, sd);
                //jakaAPI.linear_move(ref handle1, ref cartesianPose_L61, JKTYPE.MoveMode.ABS, false, sd);
            }
            else if (Action == 5)//去打印
            {
                jakaAPI.linear_move(ref handle1, ref cartesianPose_L1, JKTYPE.MoveMode.ABS, true, sd);
                jakaAPI.linear_move(ref handle1, ref cartesianPose_L2, JKTYPE.MoveMode.ABS, true, sd);
                jakaAPI.linear_move(ref handle1, ref cartesianPose_L7, JKTYPE.MoveMode.ABS, true, sd);
                jakaAPI.linear_move(ref handle1, ref cartesianPose_L8, JKTYPE.MoveMode.ABS, true, sd);
                jakaAPI.linear_move(ref handle1, ref cartesianPose_L9, JKTYPE.MoveMode.ABS, true, sd);
            }
            else if (Action == 6)//去打印地方接杯
            {
                jakaAPI.linear_move(ref handle1, ref cartesianPose_L8, JKTYPE.MoveMode.ABS, true, sd);
                jakaAPI.linear_move(ref handle1, ref cartesianPose_L7, JKTYPE.MoveMode.ABS, true, sd);
                jakaAPI.linear_move(ref handle1, ref cartesianPose_L2, JKTYPE.MoveMode.ABS, true, sd);
                jakaAPI.linear_move(ref handle1, ref cartesianPose_L1, JKTYPE.MoveMode.ABS, true, sd);
            }
            else if (Action == 7)//出杯
            {
                //akaAPI.linear_move(ref handle1, ref cartesianPose_L10, JKTYPE.MoveMode.ABS, false, 30);
                jakaAPI.linear_move(ref handle1, ref cartesianPose_L11, JKTYPE.MoveMode.ABS, false, sd);
                Thread.Sleep(100);
                jakaAPI.linear_move(ref handle1, ref cartesianPose_L12, JKTYPE.MoveMode.ABS, false, sd);
                Thread.Sleep(100);
                jakaAPI.linear_move(ref handle1, ref cartesianPose_L13, JKTYPE.MoveMode.ABS, false, sd);
                //jakaAPI.joint_move(ref handle1, ref joint_pos_L1, JKTYPE.MoveMode.ABS, false, 1);//初始点
            }
            else if (Action == 8)//右臂初始位
            {
                jakaAPI.joint_move(ref handle2, ref joint_pos_R1, JKTYPE.MoveMode.ABS, true, 1);//初始点
            }
            else if (Action == 9)//右臂接奶
            {
                Console.WriteLine("zuobi7");
                jakaAPI.linear_move(ref handle2, ref cartesianPose_R1, JKTYPE.MoveMode.ABS, true, sd);
                Console.WriteLine("zuobi8");
                jakaAPI.linear_move(ref handle2, ref cartesianPose_R2, JKTYPE.MoveMode.ABS, true, sd);
                Console.WriteLine("zuobi9");
                jakaAPI.linear_move(ref handle2, ref cartesianPose_R3, JKTYPE.MoveMode.ABS, true, sd);
            }
            else if (Action == 10)//右臂拉花静谧天鹅
            {
                //jakaAPI.linear_move(ref handle2, ref cartesianPose_R2, JKTYPE.MoveMode.ABS, true, sd);
                //jakaAPI.linear_move(ref handle2, ref cartesianPose_R1, JKTYPE.MoveMode.ABS, true, sd);
                //jakaAPI.linear_move(ref handle2, ref cartesianPose_R7, JKTYPE.MoveMode.ABS, true, sd);//初始点
                robotMoveC();
                Robot_servo_j_Swan2();
            }
            else if (Action == 11)//右臂洗杯
            {
                jakaAPI.linear_move(ref handle2, ref cartesianPose_R8, JKTYPE.MoveMode.ABS, true, 75);
                jakaAPI.linear_move(ref handle2, ref cartesianPose_R9, JKTYPE.MoveMode.ABS, true, 75);//初始点
                                                                                                      // jakaAPI.linear_move(ref handle2, ref cartesianPose_R9, JKTYPE.MoveMode.ABS, false, sd);
                jakaAPI.linear_move(ref handle2, ref cartesianPose_R5, JKTYPE.MoveMode.ABS, true, 75);
                Thread.Sleep(100);
                jakaAPI.linear_move(ref handle2, ref cartesianPose_R6, JKTYPE.MoveMode.ABS, false, 75);
            }
            else if (Action == 12)//右臂归位
            {
                jakaAPI.linear_move(ref handle2, ref cartesianPose_R5, JKTYPE.MoveMode.ABS, true, 75);
                jakaAPI.joint_move(ref handle2, ref joint_pos_R3, JKTYPE.MoveMode.ABS, true, 75);
                jakaAPI.joint_move(ref handle2, ref joint_pos_R4, JKTYPE.MoveMode.ABS, true, 75);
                jakaAPI.joint_move(ref handle2, ref joint_pos_R1, JKTYPE.MoveMode.ABS, false, 75);//初始点
            }
            else if (Action == 13)//准备接奶
            {
                Console.WriteLine("zuobi4");
                jakaAPI.joint_move(ref handle2, ref joint_pos_R1, JKTYPE.MoveMode.ABS, true, 1);//初始点
                Console.WriteLine("zuobi5");
                jakaAPI.linear_move(ref handle2, ref cartesianPose_R1, JKTYPE.MoveMode.ABS, true, sd);
            }
            else if (Action == 14)//拉花准备
            {
                jakaAPI.joint_move(ref handle1, ref joint_pos_L2, JKTYPE.MoveMode.ABS, true, 1);
            }
            else if (Action == 15)//左臂过渡点
            {
                jakaAPI.linear_move(ref handle1, ref cartesianPose_L10, JKTYPE.MoveMode.ABS, false, sd);
                Thread.Sleep(100);
            }
            else if (Action == 16)//出杯
            {
                //akaAPI.linear_move(ref handle1, ref cartesianPose_L10, JKTYPE.MoveMode.ABS, false, 30);
                jakaAPI.linear_move(ref handle1, ref cartesianPose_L11, JKTYPE.MoveMode.ABS, true, sd);
                jakaAPI.linear_move(ref handle1, ref cartesianPose_L12, JKTYPE.MoveMode.ABS, true, sd);
                jakaAPI.linear_move(ref handle1, ref cartesianPose_L13, JKTYPE.MoveMode.ABS, true, sd);
                //jakaAPI.joint_move(ref handle1, ref joint_pos_L1, JKTYPE.MoveMode.ABS, false, 1);//初始点
            }
            else if (Action == 17)//左臂直线回原点
            {
                //akaAPI.linear_move(ref handle1, ref cartesianPose_L10, JKTYPE.MoveMode.ABS, false, 30);
                jakaAPI.linear_move(ref handle1, ref cartesianPose_L14, JKTYPE.MoveMode.ABS, false, 75);
                Thread.Sleep(100);
                //jakaAPI.joint_move(ref handle1, ref joint_pos_L1, JKTYPE.MoveMode.ABS, false, 1);//初始点
            }
            else if (Action == 18)//右臂直线回原点
            {
                //akaAPI.linear_move(ref handle1, ref cartesianPose_L10, JKTYPE.MoveMode.ABS, false, 30);
                jakaAPI.linear_move(ref handle2, ref cartesianPose_R7, JKTYPE.MoveMode.ABS, false, sd);
                //jakaAPI.joint_move(ref handle1, ref joint_pos_L1, JKTYPE.MoveMode.ABS, false, 1);//初始点
            }
            else if (Action == 19)//只接咖啡
            {
                jakaAPI.linear_move(ref handle1, ref cartesianPose_L2, JKTYPE.MoveMode.ABS, false, sd);
                jakaAPI.linear_move(ref handle1, ref cartesianPose_L1, JKTYPE.MoveMode.ABS, false, sd);
                //  jakaAPI.linear_move(ref handle1, ref cartesianPose_L14, JKTYPE.MoveMode.ABS, true, 80);//初始点
                jakaAPI.linear_move(ref handle1, ref cartesianPose_L16, JKTYPE.MoveMode.ABS, true, sd);
            }
            else if (Action == 20)//未检测到杯子重新取杯2
            {
                jakaAPI.linear_move(ref handle1, ref cartesianPose_L3, JKTYPE.MoveMode.ABS, true, sd);
                jakaAPI.linear_move(ref handle1, ref cartesianPose_L4, JKTYPE.MoveMode.ABS, true, sd);
                jakaAPI.linear_move(ref handle1, ref cartesianPose_L5, JKTYPE.MoveMode.ABS, true, sd);
            }
            else if (Action == 21)//右臂拉花一叶知秋
            {
                jakaAPI.linear_move(ref handle2, ref cartesianPose_R2, JKTYPE.MoveMode.ABS, true, sd);
                jakaAPI.linear_move(ref handle2, ref cartesianPose_R1, JKTYPE.MoveMode.ABS, true, sd);
                jakaAPI.linear_move(ref handle2, ref cartesianPose_R7, JKTYPE.MoveMode.ABS, true, sd);//初始点
                robotMoveC();
                Robot_servo_j_Leaf();
            }
            else if (Action == 22)//右臂拉花郁金秋
            {
                //jakaAPI.linear_move(ref handle2, ref cartesianPose_R2, JKTYPE.MoveMode.ABS, true, sd);
                //jakaAPI.linear_move(ref handle2, ref cartesianPose_R1, JKTYPE.MoveMode.ABS, true, sd);
                //jakaAPI.linear_move(ref handle2, ref cartesianPose_R7, JKTYPE.MoveMode.ABS, true, sd);//初始点
                robotMoveC();
                Robot_servo_j_Tulip();
            }
            else if (Action == 24)//左臂直线回原点
            {
                //akaAPI.linear_move(ref handle1, ref cartesianPose_L10, JKTYPE.MoveMode.ABS, false, 30);
                jakaAPI.linear_move(ref handle1, ref cartesianPose_L14, JKTYPE.MoveMode.ABS, false, sd);
                //jakaAPI.joint_move(ref handle1, ref joint_pos_L1, JKTYPE.MoveMode.ABS, false, 1);//初始点
            }
        }
        public static void robotMoveC()
        {
            JKTYPE.OptionalCond opt = new JKTYPE.OptionalCond();
            JKTYPE.CartesianPose end_p = new JKTYPE.CartesianPose(), mid_p = new JKTYPE.CartesianPose();
            JKTYPE.CartesianPose end_p1 = new JKTYPE.CartesianPose(), mid_p1 = new JKTYPE.CartesianPose();

            end_p.tran.x = 210; end_p.tran.y = 279; end_p.tran.z = 243;
            end_p.rpy.rx = -90 / 57.3; end_p.rpy.ry = -0 / 57.3; end_p.rpy.rz = 0 / 57.3;
            mid_p.tran.x = 225; mid_p.tran.y = 264; mid_p.tran.z = 243;
            mid_p.rpy.rx = -90 / 57.3; mid_p.rpy.ry = -0 / 57.3; mid_p.rpy.rz = 0 / 57.3;

            end_p1.tran.x = 210; end_p1.tran.y = 249; end_p1.tran.z = 243;
            end_p1.rpy.rx = -90 / 57.3; end_p1.rpy.ry = -0 / 57.3; end_p1.rpy.rz = 0 / 57.3;
            mid_p1.tran.x = 195; mid_p1.tran.y = 264; mid_p1.tran.z = 243;
            mid_p1.rpy.rx = -90 / 57.3; mid_p1.rpy.ry = -0 / 57.3; mid_p1.rpy.rz = 0 / 57.3;

            JKTYPE.JointValue joint_pos = new JKTYPE.JointValue();
            joint_pos.jVal = new double[] { 24.716 / 57.3, -19.678 / 57.3, -112.003 / 57.3, 107.019 / 57.3, -71.803 / 57.3, 135.574 / 57.3 };


            JKTYPE.JointValue joint_pos1 = new JKTYPE.JointValue();
            joint_pos1.jVal = new double[] { 24.716 / 57.3, -19.678 / 57.3, -112.003 / 57.3, 107.019 / 57.3, -71.803 / 57.3, 135.574 / 57.3 };
            jakaAPI.joint_move(ref handle2, ref joint_pos1, JKTYPE.MoveMode.ABS, true, 1);//A点

            Console.WriteLine("move_j 开始");
            jakaAPI.joint_move(ref handle2, ref joint_pos1, JKTYPE.MoveMode.ABS, true, 100);
            Console.WriteLine("circle_move_开始");

            //jakaAPI.circular_move(ref handle1, ref end_p, ref mid_p, JKTYPE.MoveMode.ABS, false, 170, 50, 5, ref opt);
            //jakaAPI.circular_move(ref handle1, ref end_p1, ref mid_p1, JKTYPE.MoveMode.ABS, false, 170, 50, 5, ref opt);

            double speed = 200;
            double accel = 100;

            for (int i = 0; i < 15; i++)
            {
                jakaAPI.circular_move(ref handle2, ref end_p, ref mid_p, JKTYPE.MoveMode.ABS, false, speed, accel, 5, ref opt);
                jakaAPI.circular_move(ref handle2, ref end_p1, ref mid_p1, JKTYPE.MoveMode.ABS, false, speed, accel, 5, ref opt);
            }
            jakaAPI.circular_move(ref handle2, ref end_p, ref mid_p, JKTYPE.MoveMode.ABS, false, speed, accel, 5, ref opt);
            jakaAPI.circular_move(ref handle2, ref end_p1, ref mid_p1, JKTYPE.MoveMode.ABS, true, speed, accel, 0, ref opt);

            Console.WriteLine("circle_move_结束");
        }

        static void Robot_FilterSetting(int type, double lv, double kp = 1, double kp2 = 1)//机器人滤波器控制
        {
            int ret1;
            int ret2;

            int buffsize = 4;
            // double kp = 1;

            double kv = 1;
            double ka = 1;

            //const int type = 3;//1速度前看和3一阶低通效果较好

            //禁用滤波器
            ret1 = jakaAPI.servo_move_use_none_filter(ref handle1);
            ret2 = jakaAPI.servo_move_use_none_filter(ref handle2);
            //System.Threading.Thread.Sleep(500);

            if (type == 1)
            {
                //速度前看
                ret1 = jakaAPI.servo_speed_foresight(ref handle1, buffsize, kp);//Left
                ret2 = jakaAPI.servo_speed_foresight(ref handle2, buffsize, kp2);                                                               // ret2 = jakaAPI.servo_speed_foresight(ref handle2, buffsize, kp);//Right
            }
            else if (type == 2)
            {
                //高阶低通
                ret1 = jakaAPI.servo_move_use_joint_MMF(ref handle1, buffsize, kp, kv, ka);//Left
                ret2 = jakaAPI.servo_move_use_joint_MMF(ref handle2, buffsize, kp, kv, ka);//Right
            }
            else if (type == 3)
            {
                //一阶低通
                ret1 = jakaAPI.servo_move_use_joint_LPF(ref handle1, 3);
                ret2 = jakaAPI.servo_move_use_joint_LPF(ref handle2, lv);

            }
            else if (type == 4)
            {
                //非线性滤波
                ret1 = jakaAPI.servo_move_use_joint_NLF(ref handle1, 40, 40, 80);
                ret2 = jakaAPI.servo_move_use_joint_NLF(ref handle2, 40, 40, 80);
            }
            //System.Threading.Thread.Sleep(500);

        }
        static void Robot_Filter_LPF(double lv1, double lv2)//机器人LPF滤波器控制
        {


            //禁用滤波器
            int ret1 = jakaAPI.servo_move_use_none_filter(ref handle1);
            int ret2 = jakaAPI.servo_move_use_none_filter(ref handle2);

            //一阶低通
            int ret11 = jakaAPI.servo_move_use_joint_LPF(ref handle1, lv1);
            int ret22 = jakaAPI.servo_move_use_joint_LPF(ref handle2, lv2);



        }
    }
}
